﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using DDD;
using Xunit;

using Vector3 = DDD.Vector3<float>;

namespace DokiDokiTest {
    public class TestQuaternion {

        /// <summary>
        /// デフォルトのコンストラクターのテスト
        /// </summary>
        [Fact]
        public void Test_New0 () {
            var q = new Quaternion ();

            Assert.Equal (4, q.ComponentCount);
            Assert.Equal (false, q.IsNormalized);
            Assert.Equal (0, q.X);
            Assert.Equal (0, q.Y);
            Assert.Equal (0, q.W);
            Assert.Equal (0, q.Z);
            Assert.Equal (0, q[0]);
            Assert.Equal (0, q[1]);
            Assert.Equal (0, q[2]);
            Assert.Equal (0, q[3]);
        }

        /// <summary>
        /// AngleAxis形式のコンストラクターのテスト
        /// </summary>
        [Fact]
        public void Test_New1 () {
            var q = new Quaternion (180, 0, 0, 1);

            Assert.Equal (4, q.ComponentCount);
            Assert.Equal (true, q.IsNormalized);
            Assert.Equal (0, q.X, new FloatComparer(0.001f));
            Assert.Equal (0, q.Y, new FloatComparer (0.001f));
            Assert.Equal (1, q.Z, new FloatComparer (0.001f));
            Assert.Equal (0, q.W, new FloatComparer (0.001f));
        }

        /// <summary>
        /// 特殊なAngleAxis形式のコンストラクターのテスト
        /// </summary>
        [Fact]
        public void Test_New2 () {
            // (0,0,0,0)は特別な組み合わせで有効
            var q = new Quaternion (0, 0, 0, 0);

            Assert.Equal (4, q.ComponentCount);
            Assert.Equal (false, q.IsNormalized);
            Assert.Equal (0, q.X);
            Assert.Equal (0, q.Y);
            Assert.Equal (0, q.Z);
            Assert.Equal (1, q.W);
        }

        /// <summary>
        /// Matrix3x3をクォータニオンに変換するコンストラクターのテスト
        /// </summary>
        [Fact]
        public void Test_New3 () {
            // Matrix3x3.Rotate()で置き換えろ
            var m = Matrix4x4.Rotate (90, 1, 0, 0).Matrix3x3;
            var q = new Quaternion (m);
            var expected = new float[]{0.7071068f, 0, 0, 0.7071067f};

            Assert.Equal (expected, (float[])q, new FloatComparer(0.001f));
        }



        [Fact]
        public void Test_Normalize () {
            var q = Quaternion.Set (1, 2, 3, 4).Normalize();
            var len = (float)Math.Sqrt (30);

            Assert.Equal (true, q.IsNormalized);
            Assert.Equal (1 / len, q[0], new FloatComparer (0.001f));
            Assert.Equal (2 / len, q[1], new FloatComparer (0.001f));
            Assert.Equal (3 / len, q[2], new FloatComparer (0.001f));
            Assert.Equal (4 / len, q[3], new FloatComparer (0.001f));
        }

        [Fact]
        public void Test_Length () {
            var q = Quaternion.Set (1, 2, 3, 4);
            var expected = (float)Math.Sqrt (30);

            Assert.Equal (expected, q.Length, new FloatComparer (0.0001f));
        }

        [Fact]
        public void Test_Length2 () {
            var q = Quaternion.Set (1, 2, 3, 4);

            Assert.Equal (30, q.Length2);
        }

        [Fact]
        public void Test_Matrix3x3 () {
            var q = new Quaternion (1, 2, 3, 4);
            var expected = new float[]{ 0.9998687f, -0.01293181f, 0.009764507f,
                                        0.01299483f, 0.999895f, -0.006418638f,
                                        -0.009680477f, 0.006544684f, 0.9999318f };

            Assert.Equal (expected, (float[])q.Matrix3x3, new FloatComparer(0.001f));
        }

        [Fact]
        public void Test_AngleAxis () {
            var rotX90 = new Quaternion (90, 1, 0, 0);
            var rotY45 = new Quaternion (45, 0, -1, 0);
            var rotZ180 = new Quaternion (180, 0, 0, 1);
            var rotZ270 = new Quaternion (270, 0, 0, 1);

            Assert.Equal (90, rotX90.Angle, new FloatComparer(0.00001f));
            Assert.Equal (45, rotY45.Angle, new FloatComparer (0.00001f));
            Assert.Equal (180, rotZ180.Angle, new FloatComparer (0.00001f));
            Assert.Equal (270, rotZ270.Angle, new FloatComparer (0.00001f));
  
            Assert.Equal (new Vector3(1,0,0), rotX90.Axis);
            Assert.Equal (new Vector3(0,-1,0), rotY45.Axis);
            Assert.Equal (new Vector3(0,0,1), rotZ180.Axis);
            Assert.Equal (new Vector3(0,0,1), rotZ270.Axis);
        }

        [Fact]
        public void Test_Set1 () {
            var q = Quaternion.Set (1, 2, 3, 4, false);
            Assert.Equal (false, q.IsNormalized);
            Assert.Equal (1, q[0]);
            Assert.Equal (2, q[1]);
            Assert.Equal (3, q[2]);
            Assert.Equal (4, q[3]);

            q = Quaternion.Set (1, 2, 3, 4, true);
            var len = (float)Math.Sqrt (30);
            Assert.Equal (true, q.IsNormalized);
            Assert.Equal (1 / len, q[0], new FloatComparer (0.001f));
            Assert.Equal (2 / len, q[1], new FloatComparer (0.001f));
            Assert.Equal (3 / len, q[2], new FloatComparer (0.001f));
            Assert.Equal (4 / len, q[3], new FloatComparer (0.001f));
        }

        [Fact]
        public void Test_Set2 () {
            var q = Quaternion.Set (new float[]{1,2,3,4}, false);
            Assert.Equal (false, q.IsNormalized);
            Assert.Equal (1, q[0]);
            Assert.Equal (2, q[1]);
            Assert.Equal (3, q[2]);
            Assert.Equal (4, q[3]);

            q = Quaternion.Set (new float[]{1,2,3,4}, true);
            var len = (float)Math.Sqrt (30);
            Assert.Equal (true, q.IsNormalized);
            Assert.Equal (1 / len, q[0], new FloatComparer (0.001f));
            Assert.Equal (2 / len, q[1], new FloatComparer (0.001f));
            Assert.Equal (3 / len, q[2], new FloatComparer (0.001f));
            Assert.Equal (4 / len, q[3], new FloatComparer (0.001f));
        
        }

        [Fact]
        public void Test_Identity () {
            var q = Quaternion.Identity;
            Assert.Equal (true, q.IsNormalized);
            Assert.Equal (0, q.X);
            Assert.Equal (0, q.Y);
            Assert.Equal (0, q.Z);
            Assert.Equal (1, q.W);
        }

        [Fact]
        public void Test_Convertor () {
            var q = Quaternion.Set (1, 2, 3, 4);
            var expected = new float[] { 1, 2, 3, 4 };

            Assert.Equal (expected, (float[])q);
        }

        [Fact]
        public void Test_Conjugate () {
            var q = new Quaternion (45, 1, 1, 0).Conjugate();
            var expected = new Quaternion (-45, 1, 1, 0);

            Assert.Equal ((float[])expected, (float[])q, new FloatComparer(0.001f));
            Assert.Equal (true, q.IsNormalized);
        }

        /// <summary>
        /// クォータニオンの逆数のテスト
        /// </summary>
        [Fact]
        public void Test_Inverse1 () {
            var q = Quaternion.Set (1, 2, 3, 4).Inverse();
            var expected = Quaternion.Set (-1 / 30.0f, -2 / 30.0f, -3 / 30.0f, 4 / 30.0f);

            Assert.Equal (expected, q);
            Assert.Equal (false, q.IsNormalized);

        }

        /// <summary>
        /// クォータニオンの対数のテスト
        /// </summary>
        [Fact]
        public void Test_Log () {
            var q = new Quaternion (45, 0, 1, 1).Log();
            var expected = Quaternion.Set (0, 0.2776802f, 0.2776802f, 0);

            Assert.Equal (expected, q);
            Assert.Equal (false, q.IsNormalized);
        }

        /// <summary>
        /// クォータニオンの内積のテスト
        /// </summary>
        [Fact]
        public void Test_Dot () {
            var q1 = Quaternion.Set (1, 2, 3, 4);
            var q2 = Quaternion.Set (1, 2, 3, 4);

            Assert.Equal (30, q1.Dot (q2));
        }

        /// <summary>
        /// 2項演算子(全部)のテスト
        /// </summary>
        [Fact]
        public void Test_BinaryOperator () {
            var q1 = Quaternion.Set (1, 2, 3, 4) * 2.0f;
            var q2 = 2.0f * Quaternion.Set (1, 2, 3, 4);
            var q3 = Quaternion.Set (1, 2, 3, 4) / 2.0f;
            var q4 = Quaternion.Set (1, 2, 3, 4) + Quaternion.Set (1, 2, 3, 4);

            Assert.Equal (Quaternion.Set (2, 4, 6, 8), q1);
            Assert.Equal (Quaternion.Set (2, 4, 6, 8), q2);
            Assert.Equal (Quaternion.Set (0.5f, 1.0f, 1.5f, 2.0f), q3);
            Assert.Equal (Quaternion.Set (2, 4, 6, 8), q4);
            Assert.Equal (false, q1.IsNormalized);
            Assert.Equal (false, q2.IsNormalized);
            Assert.Equal (false, q3.IsNormalized);
            Assert.Equal (false, q4.IsNormalized);
        }

        /// <summary>
        /// 球面線形補完(Slerp)のテスト
        /// 一般的なケース
        /// </summary>
        [Fact]
        public void Test_Slerp1 () {
            var q1 = new Quaternion (0, 0, 0, 1);
            var q2 = new Quaternion (90, 0, 0, 1);
            var q3 =  Quaternion.Slerp (0, q1, q2);
            var q4 = Quaternion.Slerp(1, q1, q2);
            var q5 = Quaternion.Slerp (0.5f, q1, q2);
            var q6 = new Quaternion (45, 0, 0, 1);

            Assert.Equal (q1,q3);
            Assert.Equal (q2, q4);
            Assert.Equal (q6, q5);
            Assert.Equal (true, q3.IsNormalized);
            Assert.Equal (true, q4.IsNormalized);
            Assert.Equal (true, q5.IsNormalized);
        }

        /// <summary>
        /// 球面線形補完(Slerp)のテスト
        /// q1==q2のケース。この場合sの値に関わらずq1が返ってくる
        /// </summary>
        [Fact]
        public void Test_Slerp2 () {
            var q1 = new Quaternion (45, 1, 0, 0);
            var q2 = new Quaternion (45, 1, 0, 0);
            var q3 = Quaternion.Slerp (0.5f, q1, q2);

            Assert.Equal (q1, q3);
        }

        /// <summary>
        /// 球面線形補完(Slerp)のテスト
        /// クォータニオンの性質上「等価」な値が返ってくる。
        /// 直感に反する場合もあるので注意すること。
        /// </summary>
        [Fact]
        public void Test_Slerp3 () {
            var q1 = new Quaternion (45, 1, 0, 0);
            var q2 = new Quaternion (-45, 1, 0, 0);
            var q3 = Quaternion.Slerp (0.75f, q1, q2);

            // 現在の実装だと
            // (1,0,0), -22.5度ではなく
            // (-1,0,0), 22.5度が返ってくる
            Assert.Equal (22.5f, q3.Angle, new FloatComparer(0.01f));
            Assert.Equal (Vector3.Left, q3.Axis);
        }

        /// <summary>
        /// クォータニオンを使ったベクトルの回転のテスト
        /// </summary>
        [Fact]
        public void Test_Apply () {
            var rotX90 = new Quaternion (90, 1, 0, 0);
            var rotY90 = new Quaternion (90, 0, 1, 0);
            var rotZ90 = new Quaternion (90, 0, 0, 1);
            
            Assert.Equal ((float[])Vector3.Up, (float[])rotX90.Apply (Vector3.Forward), new FloatComparer(0.001f));
            Assert.Equal ((float[])Vector3.Back, (float[])rotY90.Apply (Vector3.Left), new FloatComparer(0.001f));
            Assert.Equal ((float[])Vector3.Right, (float[])rotZ90.Apply (Vector3.Down), new FloatComparer(0.001f));
        }

        /// <summary>
        /// クォータニオンの積のテスト
        /// </summary>
        [Fact]
        public void Test_QuaternionQuaternion () {
            var q1 = new Quaternion (90, 1, 0, 0);
            var q2 = new Quaternion (90, 0, 1, 0);

            //q2が先、q1が後
            var q3 = q1*q2;

            Assert.Equal ((float[])Vector3.Up, (float[])q3.Apply (Vector3.Right), new FloatComparer(0.001f));
            Assert.Equal (true, q3.IsNormalized);
        }

        /// <summary>
        /// クォータニオンの同値性の比較テスト
        /// </summary>
        [Fact]
        public void Test_Equals () {
            var v1 = Quaternion.Set (0, 1, 2, 3.0f);
            var v2 = Quaternion.Set (0, 1, 2, 3.02f);
            var tmp = GlobalSettings.Tolerance;

            GlobalSettings.Tolerance = 0.1f;

            Assert.True (v1 == v2);
            Assert.True (v1.Equals (v2));
            Assert.True (v1.Equals ((object)v2));
            Assert.False (v1 != v2);

            GlobalSettings.Tolerance = 0.01f;

            Assert.False (v1 == v2);
            Assert.False (v1.Equals (v2));
            Assert.False (v1.Equals ((object)v2));
            Assert.True (v1 != v2);

            GlobalSettings.Tolerance = tmp;
        }
    }
}
